#ifndef __CRT_CHASSIS_H__
#define __CRT_CHASSIS_H__
/*=============================Include========================================*/
#include "alg_math.h"
#include "alg_pid.h"
#include "alg_slope.h"
#include "dvc_djimotor.h"
/*==============================ExportedMacros================================*/
// 功率计算常量
#define POWER_CALC_CONSTANT 0.0003662109375f

// 轮组半径
const float WHEEL_RADIUS = 0.0751f;

// 轮距中心长度
const float WHEEL_TO_CORE_DISTANCE[4] = {0.23724f, 0.23724f, 0.23724f,
                                         0.23724f};
// 轮组方位角
const float WHEEL_AZIMUTH[4] = {PI / 4, 3 * PI / 4, -3 * PI / 4, -PI / 4};
/*=============================ExportedTypes==================================*/
typedef enum chassis_type {
	CHASSIS_DISABLE = 0,
	CHASSIS_OMNI,
	CHASSIS_MECANUM,
	CHASSIS_STEERING,
} chassis_type_e;

class Chassis {
	// 功率控制轮向电机PID
	PID power_limit_wheel;

	// 底盘速度值PID
	PID odom_velocity_X;
	PID odom_velocity_Y;
	PID odom_omega;

	// 斜坡函数加减速
	Slope slope_velocity_X;
	Slope slope_velocity_Y;
	Slope slope_omega;

	// 底盘陀螺仪

	// 裁判系统
	// Referee *referee;

	// 底盘转动电机
	M3508 wheel_motor[4];

	void init(float _velocity_X_max = 2.0f, float _velocity_Y_max = 2.0f,
	          float _omega_max = 4.0f);

	inline chassis_type_e get_chassis_type();
	inline float get_velocity_X_max();
	inline float get_velocity_Y_max();
	inline float get_omega_max();
	inline float get_now_power();
	inline float get_now_wheel_power();
	inline float get_target_wheel_power();
	inline float get_target_velocity_X();
	inline float get_target_velocity_Y();
	inline float get_target_omega();

	inline void set_chassis_type(chassis_type_e _type);
	inline void set_target_velocity_X(float _target_velocity_X);
	inline void set_target_velocity_Y(float _target_velocity_Y);
	inline void set_target_omega(float _target_omega);
	inline void set_now_velocity_X(float _now_velocity_X);
	inline void set_now_velocity_Y(float _now_velocity_Y);
	inline void set_now_omega(float _now_omega);

	void chassis_calc();

   protected:
	// 速度X限制
	float velocity_X_max;
	// 速度Y限制
	float velocity_Y_max;
	// 角速度限制
	float omega_max;

	// 电机理论上最大输出
	float wheel_max_output = 16384.0f;

	// 轮向电机目标值
	float target_wheel_omega[4];

	// 单个电机的功率计算中间量
	float now_tump[4];
	// 总电机的功率计算中间量
	float now_buf;
	// 当前总功率
	float now_power = 0.0f;

	// 当前轮向电机功率
	float now_wheel_power = 0.0f;
	// 可使用的轮向电机功率
	float target_wheel_power = 0.0f;

	// 底盘类型
	chassis_type_e type = CHASSIS_DISABLE;

	// 目标速度X
	float target_velocity_X = 0.0f;
	// 目标速度Y
	float target_velocity_Y = 0.0f;
	// 目标角速度
	float target_omega = 0.0f;
	// 当前速度X
	float now_velocity_X = 0.0f;
	// 当前速度Y
	float now_velocity_Y = 0.0f;
	// 当前角速度
	float now_omega = 0.0f;

	void power_calc_output();
	void power_limit();
	void speed_resolution();

	void kinematics_resolution();
	void kinematics_inverse_resolution();
	void output_to_dynamics();
	void output_to_motor();
};
/*=============================ExportedDefinitions============================*/

/*=============================ExportedFunctionDeclarations===================*/
chassis_type_e Chassis::get_chassis_type() { return type; }
float Chassis::get_velocity_X_max() { return velocity_X_max; }
float Chassis::get_velocity_Y_max() { return velocity_Y_max; }
float Chassis::get_omega_max() { return omega_max; }
float Chassis::get_now_power() { return now_power; }
float Chassis::get_now_wheel_power() { return now_wheel_power; }
float Chassis::get_target_wheel_power() { return target_wheel_power; }
float Chassis::get_target_velocity_X() { return target_velocity_X; }
float Chassis::get_target_velocity_Y() { return target_velocity_Y; }
float Chassis::get_target_omega() { return target_omega; }

void Chassis::set_chassis_type(chassis_type_e _type) { type = _type; }
void Chassis::set_target_velocity_X(float _target_velocity_X)
{
	target_velocity_X = _target_velocity_X;
}
void Chassis::set_target_velocity_Y(float _target_velocity_Y)
{
	target_velocity_Y = _target_velocity_Y;
}
void Chassis::set_target_omega(float _target_omega)
{
	target_omega = _target_omega;
}
void Chassis::set_now_velocity_X(float _now_velocity_X)
{
	now_velocity_X = _now_velocity_X;
}
void Chassis::set_now_velocity_Y(float _now_velocity_Y)
{
	now_velocity_Y = _now_velocity_Y;
}
void Chassis::set_now_omega(float _now_omega) { now_omega = _now_omega; }

#endif

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
